/*********************************************************************
// Copyright (C) 2013, PAL Robotics S.L.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions are met:
*   * Redistributions of source code must retain the above copyright notice,
*     this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above copyright
*     notice, this list of conditions and the following disclaimer in the
*     documentation and/or other materials provided with the distribution.
*   * Neither the name of PAL Robotics, Inc. nor the names of its
*     contributors may be used to endorse or promote products derived from
*     this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

#include "../include/columba_hw/columba_hw.h"

ColumbaHw::ColumbaHw(ros::NodeHandle& root_nh, ros::NodeHandle& private_nh)
  : robot_hw_loader_("controller_interface", "controller_interface::ControllerBase")
  ,running_(true)
  ,nh_(root_nh)
  , start_srv_(nh_.advertiseService("start", &ColumbaHw::start_callback, this))
  , stop_srv_(nh_.advertiseService("stop", &ColumbaHw::stop_callback, this))
  , motor_reduction_ratio(20)
{
  // Intialize raw data
  std::fill_n(pos_, 2, 0.0);
  std::fill_n(vel_, 2, 0.0);
  std::fill_n(eff_, 2, 0.0);
  std::fill_n(cmd_, 2, 0.0);

  std::vector<std::string> joint_list;
  if(private_nh.hasParam("joint"))
  {
    private_nh.getParam("joint",joint_list);
  }
  else
  {
    ROS_WARN("NO joint list!");
    joint_list.push_back("left_joint");
    joint_list.push_back("right_joint");
  }
  for(int i=0; i<joint_list.size(); i++)
  {
    std::string name = joint_list.at(i);
    hardware_interface::JointStateHandle state_handle_l(name, &pos_[i], &vel_[i], &eff_[i]);
    jnt_state_interface_.registerHandle(state_handle_l);
    hardware_interface::JointHandle vel_handle_l(jnt_state_interface_.getHandle(name), &cmd_[i]);
    jnt_vel_interface_.registerHandle(vel_handle_l);
  }

  registerInterface(&jnt_state_interface_);
  registerInterface(&jnt_vel_interface_);

  pub_velocity_ = nh_.advertise<sensor_msgs::JointState>("columba_joint_state", 10);
}

void ColumbaHw::read()
{
	
	columba_joint_state_.header.stamp = ros::Time::now();;
	columba_joint_state_.header.frame_id = "wheel";
	columba_joint_state_.velocity.resize(2);
	for (unsigned int i = 0; i < 2; ++i)
	{
		rpm_[i] = (cmd_[i] * 30 * motor_reduction_ratio) / PI;
		columba_joint_state_.velocity[i] = rpm_[i];
	} 
	pub_velocity_.publish(columba_joint_state_);
		
  std::ostringstream os;
  for (unsigned int i = 0; i < 1; ++i)
  {
    os << cmd_[i] << ", ";
  }
  os << cmd_[1];

 // ROS_INFO_STREAM("Commands for joints: " << os.str());
}

void ColumbaHw::write()
{
  if (running_)
  {  
		// Note that pos_[i] will be NaN for one more cycle after we start(),
		// but that is consistent with the knowledge we have about the state
		// of the robot.
		for (unsigned int i = 0; i < 2; ++i)
		{
			pos_[i] += cmd_[i]*getPeriod().toSec(); // update position
			vel_[i] = cmd_[i]; // might add smoothing here later
		}
  }
  else
  {
    std::fill_n(pos_, 2, std::numeric_limits<double>::quiet_NaN());
    std::fill_n(vel_, 2, std::numeric_limits<double>::quiet_NaN());
  }
}

bool ColumbaHw::start_callback(std_srvs::Empty::Request& /*req*/, std_srvs::Empty::Response& /*res*/)
{
  running_ = true;
  return true;
}

bool ColumbaHw::stop_callback(std_srvs::Empty::Request& /*req*/, std_srvs::Empty::Response& /*res*/)
{
  running_ = false;
  return true;
}

bool ColumbaHw::loadRobotHW(const std::string& name)
{
  ROS_INFO("Will load robot HW '%s'", name.c_str());

  ros::NodeHandle c_nh;
  // Constructs the robot HW
  try
  {
    c_nh = ros::NodeHandle(nh_, name);
  }
  catch(std::exception &e)
  {
    ROS_ERROR("Exception thrown while constructing nodehandle for robot HW with name '%s':\n%s", name.c_str(), e.what());
    return false;
  }
  catch(...)
  {
    ROS_ERROR("Exception thrown while constructing nodehandle for robot HW with name '%s'", name.c_str());
    return false;
  }

  boost::shared_ptr<controller_interface::ControllerBase> robot_hw;
  std::string type;
  if (c_nh.getParam("type", type))
  {
    ROS_INFO("Constructing robot HW '%s' of type '%s'", name.c_str(), type.c_str());
    try
    {
      std::vector<std::string> cur_types = robot_hw_loader_.getDeclaredClasses();
      for(size_t i=0; i < cur_types.size(); i++)
      {
        if (type == cur_types[i])
        {
          robot_hw = robot_hw_loader_.createInstance(type);
        }
      }
    }
    catch (const std::runtime_error &ex)
    {
      ROS_ERROR("Could not load class %s: %s", type.c_str(), ex.what());
    }
  }
  else
  {
    ROS_ERROR("Could not load robot HW '%s' because the type was not specified. Did you load the robot HW configuration on the parameter server (namespace: '%s')?", name.c_str(), c_nh.getNamespace().c_str());
    return false;
  }

  // checks if robot HW was constructed
  if (!robot_hw)
  {
    ROS_ERROR("Could not load robot HW '%s' because robot HW type '%s' does not exist.",  name.c_str(), type.c_str());
    return false;
  }

  // Initializes the robot HW
  ROS_DEBUG("Initializing robot HW '%s'", name.c_str());
  bool initialized;
  try
  {

  //  initialized = robot_hw->init(nh_, c_nh);
  }
  catch(std::exception &e)
  {
    ROS_ERROR("Exception thrown while initializing robot HW %s.\n%s", name.c_str(), e.what());
    initialized = false;
  }
  catch(...)
  {
    ROS_ERROR("Exception thrown while initializing robot HW %s", name.c_str());
    initialized = false;
  }

  if (!initialized)
  {
    ROS_ERROR("Initializing robot HW '%s' failed", name.c_str());
    return false;
  }
  ROS_DEBUG("Initialized robot HW '%s' successful", name.c_str());

 // robot_hw_list_.push_back(robot_hw);

//  this->registerInterfaceManager(robot_hw.get());

  ROS_DEBUG("Successfully load robot HW '%s'", name.c_str());
  return true;
}
